package proyecto1IA;

import java.awt.Point;
import java.util.ArrayList;

public class Amplitud {

    private int matrizPosiciones[][];
    private Point robot, objeto1, objeto2, lugar1, lugar2;
    private boolean encontradoObj1, encontradoObj2, entregadoLugar1, entregadoLugar2;
    private ArrayList<ArrayList<Point>> solucion;
    private int nodos;
    
    public Amplitud(int[][] matrizPosiciones, Point robot, Point objeto1, Point objeto2, Point lugar1, Point lugar2) {
        this.matrizPosiciones = matrizPosiciones;
        this.robot = robot;
        this.objeto1 = objeto1;
        this.objeto2 = objeto2;
        this.lugar1 = lugar1;
        this.lugar2 = lugar2;
        encontradoObj1 = false;
        encontradoObj2 = false;
        entregadoLugar1 = false;
        entregadoLugar2 = false;
        nodos = 1;
    }

    public ArrayList<ArrayList<Point>> controladorAmplitud() {

        Estado actual = new Estado(robot, new Point(-1, -1), 0);
        ArrayList<Estado> raizRobot = new ArrayList();
        raizRobot.add(actual);

        //Recoge alguno de los 2 objetos
        solucion = new ArrayList<ArrayList<Point>>();
        solucion.add(buscarObjetos(new ArrayList<Estado>(), raizRobot));
        solucion.get(0).remove(solucion.get(0).size() - 1);

        //Si recogió el objeto 1 mira si puede entregarlo o ir por el otro objeto 2
        if (encontradoObj1) {
            //imprimir("ENcontradoObj1");
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot = new ArrayList();
            raizRobot.add(actual);

            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, objeto2, lugar1));
            solucion.get(1).remove(solucion.get(1).size() - 1);
        } //Si recogió el 2 objeto mira si puede entregarlo o ir por el otro objeto 1
        else if (encontradoObj2) {
            //imprimir("ENcontradoObj2");
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot = new ArrayList();
            raizRobot.add(actual);
            //imprimir("Buscando objeto 1 o lugar 2, posición del robot: " + robot);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, objeto1, lugar2));
            solucion.get(1).remove(solucion.get(1).size() - 1);
            //imprimir("ENcontradoObj2 = " + encontradoObj2);
            //imprimir("confirmado encontradoOBJ2");
        }

        //Revisa si encontró un objeto o si lo entregó
        if (encontradoObj1 && entregadoLugar1) {
            //imprimir("ENcontradosObj1 y EntregadoObj1");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, objeto2, objeto2));
            solucion.get(2).remove(solucion.get(2).size() - 1);

        } else if (encontradoObj2 && entregadoLugar2) { 
            //imprimir("ENcontradosObj2 y EntregadoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, objeto1, objeto1));
            solucion.get(2).remove(solucion.get(2).size() - 1);

        } else if (encontradoObj1 && encontradoObj2) {
            //imprimir("ENcontradosObj1 y ENcontradoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            //imprimir("lugar 1: " + lugar1 + " lugar 2: "+ lugar2);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, lugar1, lugar2));
            solucion.get(2).remove(solucion.get(2).size() - 1);
        }

        //Busca la última meta a la que levar el objeto restante
        if (entregadoLugar1) {
            //imprimir("EntregadoObj1");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, lugar2, lugar2));
            
        } else if (entregadoLugar2) {
            //imprimir("EntregadoObj2");
            raizRobot = new ArrayList();
            actual = new Estado(robot, new Point(-1, -1), 0);
            raizRobot.add(actual);
            solucion.add(buscarObjetivos(new ArrayList<Estado>(), raizRobot, lugar1, lugar1));
            
        }

        return solucion;
    }

    public ArrayList<Point> buscarObjetos(ArrayList<Estado> nodosExpandidos, ArrayList<Estado> colaARevisar) {

        ArrayList<Estado> temporal = new ArrayList<Estado>();

        for (int posicion = 0; posicion < colaARevisar.size(); posicion++) {

            Estado actual = colaARevisar.get(posicion);
            if (!(actual.getPunto().x < 0 || actual.getPunto().y < 0 || actual.getPunto().x == matrizPosiciones.length || actual.getPunto().y == matrizPosiciones.length)) {
                if (!(estaEnLista(nodosExpandidos, actual)) || nodosExpandidos.isEmpty()) {
                    if (!(actual.getPunto().equals(objeto1) || actual.getPunto().equals(objeto2))) {

                        nodosExpandidos.add(actual);
                        nodos++;
                        //Arriba
                        temporal.add(new Estado(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), 0));
                        //Derecha
                        temporal.add(new Estado(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), 0));
                        //Abajo
                        temporal.add(new Estado(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), 0));
                        //Izquierda
                        temporal.add(new Estado(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), 0));

                    } else {
                        //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                        nodosExpandidos.add(actual);
                        nodos++;

                        //Revisa si el encontrado es el objeto1
                        if (actual.getPunto().equals(objeto1)) {
                            encontradoObj1 = true;
                            robot = objeto1;
                        } else if (actual.getPunto().equals(objeto2)) {
                            encontradoObj2 = true;
                            robot = objeto2;
                        }
                        ArrayList<Point> camino = new ArrayList();
                        camino.add(actual.getPunto());

                        while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                            camino.add(0, actual.getPredecesor());

                            for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {

                                if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                    actual = nodosExpandidos.get(i);
                                    break;
                                }
                            }
                        }
                        return camino;
                    }
                }
            }
            
        }
        
        return buscarObjetos(nodosExpandidos, temporal);
    }

    public ArrayList<Point> buscarObjetivos(ArrayList<Estado> nodosExpandidos, ArrayList<Estado> colaARevisar, Point objetivo1, Point objetivo2) {

        ArrayList<Estado> temporal = new ArrayList<Estado>();

        for (int posicion = 0; posicion < colaARevisar.size(); posicion++) {

            Estado actual = colaARevisar.get(posicion);
            if (!(actual.getPunto().x < 0 || actual.getPunto().y < 0 || actual.getPunto().x == matrizPosiciones.length || actual.getPunto().y == matrizPosiciones.length)) {
                if (!(estaEnLista(nodosExpandidos, actual)) || nodosExpandidos.isEmpty()) {
                    if (!(actual.getPunto().equals(objetivo1) || actual.getPunto().equals(objetivo2))) {

                        nodosExpandidos.add(actual);
                        //Arriba
                        temporal.add(new Estado(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), 0));
                        //Derecha
                        temporal.add(new Estado(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), 0));
                        //Abajo
                        temporal.add(new Estado(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), 0));
                        //Izquierda
                        temporal.add(new Estado(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), 0));

                    } else {
                        //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                        nodosExpandidos.add(actual);
                        //imprimir("Objetivo 1: " + objetivo1 + " Objetivo 2: "+ objetivo2);

                        //Revisa si el encontrado es el objeto1
                        if (actual.getPunto().equals(objetivo1)) {

                            if (objetivo1.equals(objeto1)) {
                                encontradoObj1 = true;
                                robot = objeto1;
                            } else if (objetivo1.equals(objeto2)) {
                                encontradoObj2 = true;
                                robot = objeto2;
                            } else if (objetivo1.equals(lugar1)) {
                                entregadoLugar1 = true;
                                robot = lugar1;
                            } else if (objetivo1.equals(lugar2)) {
                                entregadoLugar2 = true;
                                robot = lugar2;
                            }

                        } else if (actual.getPunto().equals(objetivo2)) {

                            if (objetivo2.equals(lugar1)) {
                                entregadoLugar1 = true;
                                robot = lugar1;
                            } else if (objetivo2.equals(lugar2)) {
                                entregadoLugar2 = true;
                                robot = lugar2;
                            }
                        }
                        
                        ArrayList<Point> camino = new ArrayList();
                        camino.add(actual.getPunto());

                        while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                            camino.add(0, actual.getPredecesor());

                            for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {
                                if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                    actual = nodosExpandidos.get(i);
                                    break;
                                }
                            }
                        }
                        return camino;
                    }
                }
            }
        }//fin del for

        return buscarObjetivos(nodosExpandidos, temporal, objetivo1, objetivo2);
    }//fin del metodo

    public boolean estaEnLista(ArrayList< Estado> lista, Estado actual) {

        for (int recorrido = 0; recorrido < lista.size(); recorrido++) {

            if (lista.get(recorrido).getPunto().x == actual.getPunto().x
                    && lista.get(recorrido).getPunto().y == actual.getPunto().y) {
                return true;
            }

        }

        return false;

    }

    public void imprimir(Object mensaje) {

        System.out.println(mensaje);

    }

    public void imprimirSinSaltoDeLinea(Object mensaje) {

        System.out.print(mensaje);

    }

    public int getNodos() {
        return nodos;
    }
    
}
